/*
 * @Description: key frames 信息发布
 * @Author: Zhijian Qiao
 * @Date: 2020-02-06 21:05:47
 */
#ifndef avp_slam_PUBLISHER_KEY_FRAMES_PUBLISHER_HPP_
#define avp_slam_PUBLISHER_KEY_FRAMES_PUBLISHER_HPP_

#include <string>
#include <deque>
#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>

#include "sensor_data/pose_data.hpp"

namespace avp_slam {
    class PathPublisher {
    public:
        PathPublisher(ros::NodeHandle &nh,
                      std::string topic_name,
                      std::string frame_id,
                      int buff_size);

        PathPublisher() = default;

        void Publish(PoseData &pose);

        void Publish(const Eigen::Matrix4f &pose, double time);

        bool HasSubscribers();

        void Reset();

    private:
        ros::NodeHandle nh_;
        ros::Publisher publisher_;
        std::string frame_id_ = "";

        uint32_t index_;
        nav_msgs::Path mPath;
    };
}
#endif